#include <ros/ros.h>
#include <std_msgs/String.h>
#include <pcl_ros/transforms.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/extract_polygonal_prism_data.h>
#include <pcl/segmentation/extract_clusters.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Pose.h>
#include <tf/transform_broadcaster.h>
#include <visualization_msgs/Marker.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <iostream> //标准输入/输出

using namespace std;

static float focus_x = 1.0;
static float focus_y = 0.0;
static float focus_z = 0.65;

static float pass_bottom = focus_z;
static float pass_top = focus_z;
static float pass_length = 0.15;
static float pass_width = 0.15;

static std::string pc_topic;
static ros::Publisher marker_pub;
static visualization_msgs::Marker line_box;
static ros::Publisher track_pose_pub;
static geometry_msgs::Pose track_pose_msg;
static ros::Publisher filtered_pub;
static tf::TransformListener *tf_listener; 
static bool bTrack = false;
static bool bFirstFrame = true;
static int nFrameCount =0;

typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;

typedef struct stBoxMarker
{
    float xMax;
    float xMin;
    float yMax;
    float yMin;
    float zMax;
    float zMin;
}stBoxMarker;
static stBoxMarker boxMarker;

typedef struct stObjectDetected
{
    string name;
    float x;
    float y;
    float z;
    float probability;
}stObjectDetected;
static stObjectDetected tmpObj;
static vector<stObjectDetected> arObj;

void DrawBoxRect(float inX, float inY, float inZ)
{
    line_box.action = 3;
    line_box.points.clear();
    marker_pub.publish(line_box);

    line_box.header.frame_id = "base_footprint";
    line_box.ns = "line_box";
    line_box.action = visualization_msgs::Marker::ADD;
    line_box.id = 1;
    line_box.type = visualization_msgs::Marker::LINE_LIST;
    line_box.scale.x = 0.01;
    line_box.color.r = 1;
    line_box.color.g = 0;
    line_box.color.b = 1;
    line_box.color.a = 1.0;

    float range = 0.04;
    // 找左右边界
    float left = inY + range;
    float right = inY - range;
    // 找前后边界
    float front = inX - range;
    float back = inX + range;
    // 上下
    float top = inZ + range;
    float bottom = inZ - range;
    
    line_box.points.clear();
    // 先画上面一圈
    geometry_msgs::Point p;
    p.z = top;
    p.x = front; p.y = left; line_box.points.push_back(p);
    p.x = front; p.y = right; line_box.points.push_back(p);
    p.x = front; p.y = right;  line_box.points.push_back(p);
    p.x = back; p.y = right; line_box.points.push_back(p);
    p.x = back; p.y = right; line_box.points.push_back(p);
    p.x = back; p.y = left; line_box.points.push_back(p);
    p.x = back; p.y = left; line_box.points.push_back(p);
    p.x = front; p.y = left; line_box.points.push_back(p);

    // 画下面一圈
    p.z = bottom;
    p.x = front; p.y = left; line_box.points.push_back(p);
    p.x = front; p.y = right; line_box.points.push_back(p);
    p.x = front; p.y = right;  line_box.points.push_back(p);
    p.x = back; p.y = right; line_box.points.push_back(p);
    p.x = back; p.y = right; line_box.points.push_back(p);
    p.x = back; p.y = left; line_box.points.push_back(p);
    p.x = back; p.y = left; line_box.points.push_back(p);
    p.x = front; p.y = left; line_box.points.push_back(p);

    // 画侧面立柱
    p.z = bottom;
    p.x = front; p.y = left; line_box.points.push_back(p);
    p.z = top;line_box.points.push_back(p);
    p.z = bottom;
    p.x = front; p.y = right; line_box.points.push_back(p);
    p.z = top;line_box.points.push_back(p);
    p.z = bottom;
    p.x = back; p.y = left; line_box.points.push_back(p);
    p.z = top;line_box.points.push_back(p);
    p.z = bottom;
    p.x = back; p.y = right; line_box.points.push_back(p);
    p.z = top;line_box.points.push_back(p);
    
    marker_pub.publish(line_box);
}

void SortObjectsInTrack()
{
    int nNum = arObj.size();
    if (nNum == 0)
        return;
    int nTrackIndex =0;
    if(nNum > 0)
    {
        // 找出离跟踪位置最近的
         float minObjDist =sqrt((arObj[0].x - focus_x) * (arObj[0].x - focus_x) + (arObj[0].y - focus_y) * (arObj[0].y - focus_y));
        for(int n = 0; n<nNum; n++)
        {
            float curDist =sqrt((arObj[n].x - focus_x) * (arObj[n].x - focus_x) + (arObj[n].y - focus_y) * (arObj[n].y - focus_y));
            if(curDist < minObjDist)
            {
                // 记录下标
                nTrackIndex = n;
                minObjDist = curDist;
            }
         }
    }
    // 更新追踪坐标
    focus_x = arObj[nTrackIndex].x;
    focus_y = arObj[nTrackIndex].y;
    ROS_WARN("[object_track_node]追踪目标刷新 focus_x= %.2f  focus_y = %.2f ",focus_x,focus_y);
    DrawBoxRect(focus_x,focus_y,focus_z);
    // 搜索完毕，发送消息
    track_pose_msg.position.x = focus_x;
    track_pose_msg.position.y = focus_y;
    track_pose_msg.position.z = focus_z;
    track_pose_pub.publish(track_pose_msg);
}

static bool bNewPC = false;
static sensor_msgs::PointCloud2 pc_footprint;
void ProcCloudCB(const sensor_msgs::PointCloud2 &input)
{
    nFrameCount ++;
    //ROS_INFO("box_track_node ProcCloudCB ...");
    if(bTrack == false)
        return;

    if(bNewPC == true)
        return;

    //to footprint
    bool res = tf_listener->waitForTransform("/base_footprint", input.header.frame_id, input.header.stamp, ros::Duration(5.0)); 
    if(res == false)
    {
        return;
    }
    pcl_ros::transformPointCloud("/base_footprint", input, pc_footprint, *tf_listener);
    bNewPC = true;

    
    
    ros::spinOnce();
}

void ObjectTrackCB(const geometry_msgs::Pose &msg)
{
    focus_x = msg.position.x;
    focus_y = msg.position.y;
    focus_z = msg.position.z;
    bTrack = true;
    ROS_WARN("[object_track_node] 开始追踪！");
}

void BehaviorCB(const std_msgs::String::ConstPtr &msg)
{
    int nFindIndex = 0;
    nFindIndex = msg->data.find("object_track stop");
    if( nFindIndex >= 0 )
    {
        ROS_WARN("[object_track_node] 停止追踪");
        bTrack = false;
    }
}

int main(int argc, char **argv)
{
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "object_track_node");
    tf_listener = new tf::TransformListener();

    ros::NodeHandle nh_param("~");
    nh_param.param<std::string>("topic", pc_topic, "/camera/depth/color/points");
    // nh_param.param<std::string>("topic", pc_topic, "/camera/depth_registered/points");

    ros::NodeHandle nh;
    ros::Subscriber pc_sub = nh.subscribe(pc_topic, 2 , ProcCloudCB);
    ros::Subscriber pose_sub = nh.subscribe("/object_track_target", 1 , ObjectTrackCB);
    ros::Subscriber beh_sub = nh.subscribe("/wpv4/behaviors", 10, BehaviorCB);

    marker_pub = nh.advertise<visualization_msgs::Marker>("/object_track_marker", 10);
    track_pose_pub = nh.advertise<geometry_msgs::Pose>("/object_track_pose", 10);
    // filtered_pub = nh.advertise<PointCloud> ("/segmented_filtered",1);
    
    ros::Rate r(30);
    int nCount = 0;
    while(ros::ok())
    {
        if(bNewPC == true)
        {
            //source cloud
            pcl::PointCloud<pcl::PointXYZRGB> cloud_src;
            pcl::fromROSMsg(pc_footprint , cloud_src);
            //ROS_INFO("[ProcCloudCB]cloud_src size = %d",(int)(cloud_src.size())); 
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_source_ptr;
            cloud_source_ptr = cloud_src.makeShared(); 

            // 截取盒子范围的点云
            pcl::PassThrough<pcl::PointXYZRGB> pass;//设置滤波器对象
            pass.setInputCloud (cloud_source_ptr);
            pass.setFilterFieldName ("z");
            pass_bottom = focus_z ;//- 0.2;
            pass_top = pass_bottom + 0.05;
            pass.setFilterLimits (pass_bottom, pass_top);
            pass.filter (*cloud_source_ptr);
            pass.setFilterFieldName ("x");
            float front = focus_x - pass_length/2;
            float back = focus_x + pass_length/2;
            pass.setFilterLimits (front, back);
            pass.filter (*cloud_source_ptr);
            pass.setFilterFieldName ("y");
            float left = focus_y + pass_width/2;
            float right = focus_y -pass_width/2;
            pass.setFilterLimits (right, left);
            pass.filter (*cloud_source_ptr);
            // filtered_pub.publish(cloud_source_ptr);
            int nSizeCloudPoints = cloud_source_ptr->size();
            ROS_INFO("[object_track_node] 分割出物体点云 = %d",nSizeCloudPoints); 
            if(cloud_source_ptr->points.size() == 0)
            {
                bNewPC = false;
                continue;
            }
            
            ///////////////////////////////////////////////////////////////////////////////////////////////////////////
            // [方法二] 直接统计质心
            int nNum = cloud_source_ptr->points.size();
            float points_x_sum = 0;
            float points_y_sum = 0;
            float points_z_sum = 0;
            for(int i=0;i<nNum;i++)
            {
                points_x_sum += cloud_source_ptr->points[i].x;
                points_y_sum += cloud_source_ptr->points[i].y;
                // points_z_sum += cloud_source_ptr->points[i].z;
            }
            focus_x = points_x_sum/nNum;
            focus_y = points_y_sum/nNum;
            // 发送给抓取节点
            track_pose_msg.position.x = focus_x;
            track_pose_msg.position.y = focus_y;
            track_pose_msg.position.z = focus_z;
            track_pose_pub.publish(track_pose_msg);
            ROS_WARN("[object_track_node]追踪目标 = ( %.2f, %.2f)",focus_x,focus_y);
            DrawBoxRect(focus_x,focus_y,focus_z);
            ////////////////////////////////////////////////////////////////////////////////////////////////////////////
            // [方法一] 对截取后的点云进行欧式距离分割
            // static pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
            // static std::vector<pcl::PointIndices> cluster_indices;	        // 点云团索引
            // cluster_indices.clear();
            // static pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;    // 欧式聚类对象
            // tree->setInputCloud (cloud_source_ptr);		                    // 输入截取后的点云
            // ec.setClusterTolerance (0.02);			                        // 设置近邻搜索的搜索半径为2cm
            // ec.setMinClusterSize (100);			                            // 设置一个聚类需要的最少的点数目为200
            // ec.setMaxClusterSize (10000);			                        // 设置一个聚类需要的最大点数目为10000
            // ec.setSearchMethod (tree);			                            // 设置点云的搜索机制
            // ec.setInputCloud (cloud_source_ptr);                            // 输入截取后的点云
            // ec.extract (cluster_indices);                                   // 执行欧式聚类分割

            // arObj.clear();
            // int nObjCnt = 0;
            // int object_num = cluster_indices.size();                        // 分割出的点云团个数
            // ROS_INFO("[object_track_node] 物体数量 = %d",object_num);
            // for(int i = 0 ; i < object_num ; i ++)
            // {
            //     int point_num =  cluster_indices[i].indices.size();         // 点云团i中的点数
            //     float points_x_sum = 0;
            //     float points_y_sum = 0;
            //     float points_z_sum = 0;
            //     bool bFirstPoint = true;
            //     for(int j = 0 ; j < point_num ; j ++)
            //     {
            //         int point_index = cluster_indices[i].indices[j];
            //         points_x_sum += cloud_source_ptr->points[point_index].x;
            //         points_y_sum += cloud_source_ptr->points[point_index].y;
            //         points_z_sum += cloud_source_ptr->points[point_index].z;
            //         pcl::PointXYZRGB p = cloud_source_ptr->points[point_index];
            //         if(bFirstPoint == true)
            //         {
            //             boxMarker.xMax = boxMarker.xMin = p.x;
            //             boxMarker.yMax = boxMarker.yMin = p.y;
            //             boxMarker.zMax = boxMarker.zMin = p.z;
            //             bFirstPoint = false;
            //         }

            //         if(p.x < boxMarker.xMin) { boxMarker.xMin = p.x;}
            //         if(p.x > boxMarker.xMax) { boxMarker.xMax = p.x;}
            //         if(p.y < boxMarker.yMin) { boxMarker.yMin = p.y;}
            //         if(p.y > boxMarker.yMax) { boxMarker.yMax = p.y;}
            //         if(p.z < boxMarker.zMin) { boxMarker.zMin = p.z;}
            //         if(p.z > boxMarker.zMax) { boxMarker.zMax = p.z;}
            //     }
            //     std::ostringstream stringStream;
            //     stringStream << "obj_" << nObjCnt;
            //     std::string obj_id = stringStream.str();
            //     float object_x = boxMarker.xMin;
            //     float object_y = (boxMarker.yMin+boxMarker.yMax)/2;
            //     float object_z = focus_z;
            //     // DrawText(obj_id,0.06, boxMarker.xMax,object_y,boxMarker.zMax + 0.05, 1,0,1);
            //     tmpObj.name = obj_id;
            //     tmpObj.x = object_x;
            //     tmpObj.y = object_y;
            //     tmpObj.z = object_z;
            //     tmpObj.probability = 1.0f;
            //     arObj.push_back(tmpObj);

            //     nObjCnt++;
            //     ROS_INFO("[物品_%d] xMin= %.2f yMin = %.2f yMax = %.2f  z = %.2f pnt= %d",i,boxMarker.xMin, boxMarker.yMin, boxMarker.yMax,boxMarker.zMin,point_num);
            // }
            // if(nObjCnt > 0)
            // {
            //     // 找出离锁定位置最近的，更新追踪坐标
            //     SortObjectsInTrack();
            // }
            ///////////////////////////////////////////////////////////////////////////////////////////////////////////
            bNewPC = false;
        }
        nCount ++;
        if(nCount >= 30)
        {
            // ROS_INFO("[box_track_node] 点云帧率= %d", nFrameCount);
            nFrameCount = 0;
            nCount = 0;
        }
        ros::spinOnce();
        r.sleep();
    }

    delete tf_listener;

    return 0;
}